#include <iostream>

#include "avr-core.h"
#include "bit-field.h"
#include "exceptions.h"

using namespace std;

AVRCore::AVRCore (FlashMemory& program_memory, uint16_t address_map)
  : DataBusMember (address_map),
    _program_memory (program_memory)
{
  reset ();
}

void AVRCore::reset ()
{
  _pc = 0;
  _sp = 0;
  _sreg = 0;
  _cur_op = 0;
  _cskip = 0;
}

void AVRCore::add_operation (CoreOperation* op)
{
  _ops.push_back (op);
}

void AVRCore::clock_tick ()
{
  if (_cskip == 0) {
    uint16_t inst = _program_memory[_pc];

    vector<CoreOperation*>::iterator beg = _ops.begin ();
    vector<CoreOperation*>::iterator end = _ops.end ();

    for (; beg != end; ++beg) {
      if ((inst & (*beg)->mask ()) == (*beg)->opcode ()) {
        _cur_op = *beg;
        break;
      }
    }

    if (beg == end)
      throw CoreException (_pc, inst);

    _cskip = _cur_op->execute (inst, *this);
  }

  --_cskip;
}

uint8_t AVRCore::read (uint16_t address)
{
  int reg = address - get_address_base ();

  switch (reg) {
    case 0:
      return get_byte<uint16_t> (_sp, 0);

    case 1:
      return get_byte<uint16_t> (_sp, 1);

    case 2:
      return _sreg;

    default:
      break;
  }

  return DataBusMember::read (address);
}

void AVRCore::write (uint16_t address, uint8_t data)
{
  int reg = address - get_address_base ();

  switch (reg) {
    case 0:
      set_byte<uint16_t> (_sp, 0, data);
      break;

    case 1:
      set_byte<uint16_t> (_sp, 1, data);
      break;

    case 2:
      _sreg = data;
      break;

    default:
      DataBusMember::write (address, data);
  }
}

void AVRCore::dump_status (ostream& os)
{
  ios::fmtflags flgs = os.flags ();

  os.setf (ios::showbase | ios::left);
  os << "PC: ";
  os.width (7);
  os << hex << (int)_pc;
  os.width (0);
  os << "SP: ";
  os.width (7);
  os << hex << (int)_sp;
  os.width (0);
  os << "SR: ";

  os << (get_bit<uint8_t> (_sreg, 7) ? 'I' : 'i');
  os << (get_bit<uint8_t> (_sreg, 6) ? 'T' : 't');
  os << (get_bit<uint8_t> (_sreg, 5) ? 'H' : 'h');
  os << (get_bit<uint8_t> (_sreg, 4) ? 'S' : 's');
  os << (get_bit<uint8_t> (_sreg, 3) ? 'V' : 'v');
  os << (get_bit<uint8_t> (_sreg, 2) ? 'N' : 'n');
  os << (get_bit<uint8_t> (_sreg, 1) ? 'Z' : 'z');
  os << (get_bit<uint8_t> (_sreg, 0) ? 'C' : 'c');
  os << endl;

  os.flags (flgs);
}

void AVRCore::execute_instruction (uint16_t instruction)
{
  _program_memory[_pc] = instruction;
  uint16_t pc = _pc;

  while (_pc == pc)
    clock_tick ();
}
